#include <Transform.h>


/************************************
/*              Public
/************************************/

    /************************************
    /*          Constructors
    /************************************/

        Transform::Transform(){
            InitializeDirectionsVectors();
            Initialize();
            m_WorldMatrix       =   new Matrix4();
        }

        Transform::Transform(Matrix4 &  mat){
            InitializeDirectionsVectors();
            Initialize();
            m_WorldMatrix   =   &mat;
        }

        Transform::Transform(Matrix4 *  mat){
            InitializeDirectionsVectors();
            Initialize();
            m_WorldMatrix   =   mat;
        }

        void Transform::Initialize(){
            HasParent   =false;
            IsWorld     =true;
            IsView      =false;
            valuelol    =   0.0f;
            m_EulerAngles       =   new vec4();
            m_Position          =   new vec4();
            m_RotationMatrix    =   new Matrix4();
            m_PositionMatrix    =   new Matrix4();
            m_LocalMatrix       =   new Matrix4();
        }

    /************************************
    /*              Methods
    /************************************/

        void Transform::LookAt(vec4 &vec){

            //  Je récupère le vecteur entre la position de l'objet et le point "visé"
            vec4    target      =   vec-*m_Position;
            target              =   target.Normalize();
            target.w            =   1;



            //  Je calcule le produit vectoriel du vecteur "target" et de "forward" pour trouver l'axe de rotation
            vec4 axis           =   vec4::Cross(vec4::Forward, target);
            axis                =   axis.Normalize();
            axis.w              =   1;


            //  Je calcule l'angle entre le vecteur target et le vecteur "forward"
            float value         =   acos(vec4::Dot(vec4::Forward,target));


            //  Je récupère la nouvelle valeur du vecteur Up qu'il faudra modifier
            RotationMatrix()    =   Quaternion(axis.x, axis.y, axis.z,value).RotationMatrix();
            vec4 newup          =   RotationMatrix()*vec4::Up;

            newup.Normalize();
            newup.w=1;

            Matrix4 mat =  Matrix4();
            float a =   target.x;
            float b =   target.y;
            float c =   target.z;

            mat[0]=pow(b,2)+pow(c,2);   mat[1]=-a*b;                mat[2]=-a*c;
            mat[4]=-b*a;                mat[5]=pow(a,2)+pow(c,2);   mat[6]=-b*c;
            mat[8]=-c*a;                mat[9]=-c*b;                mat[10]=pow(a,2)+pow(b,2);

            vec4 yaxis  =   mat*vec4::Up;
            vec4 up     =   newup;



            float   d   =   -vec4::Dot(target,Position());
            float val   =   (target.x*vec4::Up.x)+(target.y*vec4::Up.y)+(target.z*vec4::Up.z);
            float val2  =   d;

            float k= (-val2)/val;
            vec4 upositionAxisProjeted= vec4::Up*k;

            //upositionAxisProjeted.Print();

            upositionAxisProjeted   =   Position()-upositionAxisProjeted;
            //upositionAxisProjeted   =   upositionAxisProjeted.Normalize();


            float angle = acos(vec4::Dot(up.Normalize(), upositionAxisProjeted.Normalize()));
            RotationMatrix().Identity();
            float valsupertest;

           /* if(target.z>0){
                valsupertest=-1.0f;

            }
            else{
                valsupertest=1.0f;
                angle=angle-3.14;
            }

            if(target.y>0){
                angle=angle-3.14;
            }*/
 angle=angle-3.14;
            RotationMatrix()    =       (
                                            (Quaternion(axis.x, axis.y, axis.z,value)
                                            *
                                            (Quaternion(0.0f,0.0f,valsupertest,angle))

                                         )).RotationMatrix();

            Update();

        }

        /************************************
        /*              Get
        /************************************/


        /************************************
        /*              Set
        /************************************/
        vec4    Transform::WorldPosition(){
            if(HasParent){
                return (Parent().WorldMatrix()*Position());
            }else{
                return Position();
            }
        }


        void Transform::Update()
        {
            if(IsView){
                WorldMatrix() =    RotationMatrix()*TranslationMatrix() ;
            }else{
                LocalMatrix()   = TranslationMatrix()*RotationMatrix() ;

                if(HasParent && IsWorld){
                    WorldMatrix()   =   Parent().WorldMatrix()*TranslationMatrix();
                }else{
                    WorldMatrix()   =   LocalMatrix();
                }
            }

            m_Forward           =   RotationMatrix()* vec4::Forward ;
            m_Up                =   RotationMatrix()* vec4::Up;
            m_Right             =   RotationMatrix()* vec4::Right;

            m_Forward= m_Forward.Normalize();
            m_Up    =m_Up .Normalize();
            m_Forward.w     =   1;
            m_Up.w          =   1;
        }

        void Transform::Translate(float x, float y, float z)
        {
            Position()             =   vec4(Position().x+x, Position().y+y, Position().z+z, 1.0);
            TranslationMatrix()    =   (Matrix4::Translation(Position().x, Position().y, Position().z));
            Update();
        }
        void Transform::Translate(const vec4 & vec)
        {
            Translate(vec.x, vec.y, vec.z);
        }

        void Transform::Translation(float x, float y, float z)
        {
            Position()= vec4(x, y, z, 1.0);
            TranslationMatrix() = (Matrix4::Translation(Position().x, Position().y, Position().z));
            Update();
        }

        void Transform::Translation(vec4 vec)
        {
            Position()= vec4(vec.x, vec.y, vec.z, 1.0);
            TranslationMatrix() = (Matrix4::Translation(Position().x, Position().y, Position().z));
            Update();
        }
        void Transform::RotationEuler(float x, float y, float z)
        {
            EulerAngles()       = vec4(x,y,z,1.0);
            RotationMatrix()    = (Quaternion::Euler(EulerAngles().x,EulerAngles().y,EulerAngles().z)).RotationMatrix();
            Update();
        }

        void Transform::RotateEuler(float x, float y, float z)
        {
            EulerAngles() = vec4(EulerAngles().x+x,EulerAngles().y+y,EulerAngles().z+z,1.0f);
            RotationMatrix()= (Quaternion::Euler(EulerAngles().x,EulerAngles().y,EulerAngles().z)).RotationMatrix();
            Update();
        }

        void Transform::RotateAroundAxis(float x, float y, float z, float teta)
        {
            Quaternion quat(x,y,z,teta);
            RotationMatrix()=quat.RotationMatrix();
            Update();
        }

        void Transform::Clear()
        {
            Position()      =   vec4::Zero;
            EulerAngles()   =   vec4::Zero;
        }

        /************************************
        /*              Get
        /************************************/

            Transform & Transform::Parent(){
                return *m_Parent;
            }

             vec4 & Transform::Forward(){
                return m_Forward;
            }

            const vec4 & Transform::Up(){
                return m_Up;
            }

            const vec4 & Transform::Right(){
                return m_Right;
            }

            Matrix4 &   Transform::WorldMatrix(){
                return *m_WorldMatrix;
            }

            Matrix4&    Transform::LocalMatrix(){
                return *m_LocalMatrix;
            }

            Matrix4&    Transform::RotationMatrix(){
                return *m_RotationMatrix;
            }

            Matrix4&    Transform::TranslationMatrix(){
                return *m_PositionMatrix;
            }

            vec4&       Transform::Position(){
                return *m_Position;
            }

            vec4&       Transform::EulerAngles(){
                return *m_EulerAngles;
            }


        /************************************
        /*              Set
        /************************************/
            void Transform::Parent(Transform &parent){
                HasParent   =   true;
                m_Parent=&parent;
            }

    /************************************
    /*          Private
    /************************************/

        void Transform::InitializeDirectionsVectors(){
            m_Forward   =    vec4(0.0f, 0.0f, -1.0f);
            m_Up        =    vec4(0.0f, 1.0f, 0.0f);
            m_Right     =    vec4(1.0f, 0.0f, 0.0f);
        }

        void Transform::UpdateDirectionsVector(){

        }



